#include "mbt/tracker.hh"

class RBOTTracker : public Tracker {
public:
	//RBOTTracker(const cv::Matx33f& K, const cv::Matx14f& distCoeffs, std::vector<Object3D*>& objects);
	RBOTTracker(const cv::Matx33f& K, std::vector<Object3D*>& objects);
	~RBOTTracker();

	virtual void ToggleTracking(cv::Mat& frame, int objectIndex, bool undistortFrame = true) override;
	virtual void EstimatePoses(cv::Mat& frame, bool check_lost) override;

protected:
	virtual void Track(std::vector<cv::Mat>& imagePyramid, std::vector<Object3D*>& objects, int runs = 1);
	void RunIteration(std::vector<Object3D*>& objects, const std::vector<cv::Mat>& imagePyramid, int level);
	void ComputeJacobians(Object3D* object, const cv::Mat& frame, const cv::Mat& depth, const cv::Mat& depthInv, const cv::Mat& sdt, const cv::Mat& xyPos, const cv::Rect& roi, const cv::Mat& mask, int m_id, int level, cv::Matx66f& wJTJ, cv::Matx61f& JT, int threads);

private:

	SignedDistanceTransform2D* SDT2D;

	int tmp;

	void relocalize(Object3D* object, std::vector<cv::Mat>& imagePyramid);

	float evaluateEnergyFunction(Object3D* object, const cv::Mat& binned, int level, int threads);

	float evaluateEnergyFunction(Object3D* object, const cv::Mat& mask, const cv::Mat& depth, const cv::Mat& binned, int level, int threads);

	float evaluateEnergyFunction(TCLCHistograms* tclcHistograms, const std::vector<cv::Point3i>& centersIDs, const cv::Mat& binned, const cv::Mat& heaviside, const cv::Rect& roi, int offsetX, int offsetY, int level, int threads);

	float evaluateEnergyFunction_local(TCLCHistograms* tclcHistograms, const std::vector<cv::Point3i>& centersIDs, const cv::Mat& binned, const cv::Mat& heaviside, const cv::Rect& roi, int offsetX, int offsetY, int level);
};